import rospy
from geometry_msgs.msg import PoseStamped
from mavros_msgs.msg import State
from geometry_msgs.msg import TwistStamped
from sensor_msgs.msg import Imu
from mavros_msgs.msg import RCOut
from sensor_msgs.msg import BatteryState

def state_cb(data):
    # rospy.INFO("state ",data)
    print("state--------")
    print(data)

def pose_cb(data):
    print("pose------------")
    print(data)

def velo_cb(data):
    print("velocity------------")
    print(data)

def imu_cb(data):
    print("imu------------")
    print(data)

def rcOut_cb(data):
    print("moter------------")
    print(data)

def batt_cb(data):
    print("battery------------")
    print(data)

if __name__ == "__main__":
    rospy.init_node("getmsg_node_py")
    state_sub = rospy.Subscriber("mavros/state", State, callback = state_cb)
    #     位置信息		姿态信息
    # /mavros/local_position/pose [geometry_msgs/PoseStamped]		
    # /mavros/local_position/pose_cov [geometry_msgs/PoseWithCovarianceStamped]
    pose_sub = rospy.Subscriber("mavros/local_position/pose", PoseStamped, callback = pose_cb)
    # 速度信息
    # /mavros/local_position/velocity_body [geometry_msgs/TwistStamped]
    # /mavros/local_position/velocity_body_cov [geometry_msgs/TwistWithCovarianceStamped]
    # /mavros/local_position/velocity_local [geometry_msgs/TwistStamped]
    velo_sub = rospy.Subscriber("mavros/local_position/velocity_body", TwistStamped, callback = velo_cb)
    # 传感器信息
    # /mavros/imu/data [sensor_msgs/Imu]				IMU数据
    # /mavros/imu/data_raw [sensor_msgs/Imu]			IMU裸数据
    # /mavros/imu/diff_pressure [sensor_msgs/FluidPressure]		气压差传感器
    # /mavros/imu/mag [sensor_msgs/MagneticField]					罗盘数据
    # /mavros/imu/static_pressure [sensor_msgs/FluidPressure]		气压计
    # /mavros/imu/temperature_baro [sensor_msgs/Temperature]
    # /mavros/imu/temperature_imu [sensor_msgs/Temperature]
    imu_sub = rospy.Subscriber("mavros/imu/data_raw", Imu, callback = imu_cb)
    # 电机转速
    # /mavros/rc/RCout [mavros_msgs/RCout]							电机数据
    rcout_sub = rospy.Subscriber("mavros/rc/out", RCOut, callback = rcOut_cb)

    # 电池电压
    # /mavros/battery [sensor_msgs/BatteryState]
    batt_sub = rospy.Subscriber("mavros/battery", BatteryState, callback = batt_cb)
    rospy.spin()